//TODO Control Tortuosity of solution
//TODO Rank different solutions
//TODO 2D to 3D
//TODO Handling polyhedral objects

/************************************************************************************
    This program calculates the shortest distance between two points using A* search
    
    Copyright (C) 2013 Aditya Nag Matsa 
    
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*************************************************************************************/

#include "astar.h"

int main()
{
    //  This stores the problem set
	FILE *map = fopen("./map", "r");
	
	//	This stores the final solution
	FILE *result = fopen("./result", "r+");

	//	Initially result is simply a copy of map
	char ch;
	while ( (ch = fgetc(map)) != EOF)
		fputc(ch, result);
	
	int i, j;
	    
    //  Initializing the start and goal coordinates
	Point start = {0, 0, 0.0, 0.0, NULL};
	Point goal 	= {39, 39, 0.0, 0.0, NULL};
	
	//  Allocating the closed and open sets
	Node *closedset	= (Node *) malloc (sizeof(Node));
	Node *openset 	= (Node *) malloc (sizeof(Node));
	
	//  Initializing the openset
	openset->location = &start;
	openset->next = NULL;
	
	//  Initialize the g and f scores of the start coordinate
	start.g = 0.0;
	start.f = start.g + heuristic(&start, &goal);

    //  Dummy variables	
	Point *current;
	Point *neighbor;
	float tentative_g;
	
	FILE *evaluated_openset = fopen("./openset", "r+");
	FILE *evaluated_closedset = fopen("./closedset", "r+");
	
	while ( nodeCount(openset) )                //  Loop till the openset is empty
	{
		current = priority(openset);            //  Choose the point from the openset which has the least f score
		
		if ( equalElements(current, &goal) )    //  If the chosen point is the goal, then stop and print the solution
		{
			goal = *current;
			reconstructPath (&goal, result);
			printf("Distance Travelled: %f\n", goal.f);
			
			print(openset, evaluated_openset);
			print(closedset, evaluated_closedset);
	
			return 0;
		}
		
		openset = nodeDelete(current, openset);         //  Else remove the point from the openset
		closedset = nodeAppend(current, closedset);     //  And add it to the closedset
		
		neighbor = (Point *) malloc (8*sizeof(Point));
		neighborNodes(current, neighbor, map);   //  Find the POSSIBLE neighbors of the current point (8 possible)
		
		for (i=0; i<8; i++)                             
		{
			if ( (neighbor+i)->x == -1 && (neighbor+i)->y == -1 )   //  For each such neighbor, enter this loop
				continue;
			
			if ( nodeSearch( neighbor+i, closedset) )               //  If the neighbor is in the closedset, then choose another neighbor
				continue;
			
			tentative_g = current->g + distance( current, neighbor+i );     
			
			if ( !nodeSearch( neighbor+i, openset ) || tentative_g < (neighbor+i)->g )  //  If the neighbor is NOT in the openset or if a better
			{                                                                           //  solution exists, then:
				(neighbor+i)->from = current;                                           //  Mark the point from which you reached this neighbor
				(neighbor+i)->g = tentative_g;                                          //  Update the g and f scores of this neighbor
				(neighbor+i)->f = (neighbor+ i)->g + heuristic( neighbor+i, &goal);
				
				if ( !nodeSearch( neighbor+i, openset) )                                //  If the neighbor is NOT in the openset, then
					openset = nodeAppend( neighbor+i, openset);                         //  add it to the openset
			}
		}
	}
	
	fclose(map);
	fclose(result);
	fclose(evaluated_openset);
	fclose(evaluated_closedset);
	
	printf("No optimal routes.\n");                     //  If no optimal solution exists, return failure
	return -1;
}
